#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
#include <iostream>


void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) {
    std::cout << "Linear Velocity: x=" << msg->twist.twist.linear.x 
              << ", y=" << msg->twist.twist.linear.y 
              << ", z=" << msg->twist.twist.linear.z << std::endl;
    std::cout << "Angular Velocity: x=" << msg->twist.twist.angular.x 
              << ", y=" << msg->twist.twist.angular.y 
              << ", z=" << msg->twist.twist.angular.z << std::endl;
    std::cout << "------------------------" << std::endl;
}

int main(int argc, char **argv) {
    ros::init(argc, argv, "display_velocity");
    ros::NodeHandle nh;
    
    // 修改订阅的topic为/odom，因为/cmd_vel在topic列表中不存在
    ros::Subscriber sub = nh.subscribe("/odom", 10, odomCallback);
    
    ros::spin();
    
    return 0;
}
